#include "FSM/State_Wheel.h"

State_Wheel::State_Wheel(CtrlComponents* ctrlcomp):State_FixedStand(ctrlcomp)
{
    //指定{hip}下目标足端位置
    Vec34 target_feet_end_posB;     //4条腿在{hip}下足端位置
    double a=HIP_LENGTH;
    std::cout<<"hip length:"<<HIP_LENGTH<<std::endl;
    double b=-a;
    double h=-0.3;      //机身离地高度
    double c=0.2;       //足端x轴偏移
    double d=-c;
    target_feet_end_posB    <<  c,c,d,d,
                                b,a,b,a,
                                h,h,h,h;

    //计算目标关节角
    _targetQ=_ctrlComp->robotModel->getQ(target_feet_end_posB,FrameType::HIP);

    //修改状态名和状态枚举
    _stateNameString="wheel mode";
    _stateName=FSMStateName::WHEEL;
}